﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Threading;
using Microsoft.Devices.Sensors;
using Microsoft.Xna.Framework;
using S = StaticView.Algorithm;

namespace StaticView.Input
{
    class InternalAccelerometer : ISensor
    {
        Accelerometer acce;
        CalibrationCallback callback;
        int id;

        public InternalAccelerometer(CalibrationCallback callback, int id)
        {
            acce = new Accelerometer();
            this.callback = callback;
            this.id = id;
        }

        // Calibrate the sensor, time in ms
        // Return the value of gravity
        public void Calibrate(object param)
        {
            int time = (int)param;

            acce.Start();
            int timeBtwUpdates = acce.TimeBetweenUpdates.Milliseconds;
            // Get samples, calculate the sum
            List<Vector3> results = new List<Vector3>();
            Vector3 sum = new Vector3(0, 0, 0);
            for (int t = 0; t < time; t += timeBtwUpdates)
            {
                Vector3 v = getValue();
                results.Add(v);
                sum += v;
                Thread.Sleep(timeBtwUpdates);
            }
            // Calculate the mean, variance
            sum /= results.Count;
            S.Matrix mean = new S.Matrix(sum);
            S.Matrix cov = new S.Matrix(3); // Covariance matrix
            foreach (Vector3 v in results)
            {
                S.Matrix var = (new S.Matrix(v)) - mean;
                cov += (var * var.Transpose());
            }
            cov /= results.Count;
            Noise = cov;
            callback(id, true, new float[] { sum.X, sum.Y, sum.Z });
        }

        public Vector3 getValue()
        {
            return acce.CurrentValue.Acceleration;
        }
        public long getTime()
        {
            return acce.CurrentValue.Timestamp.Ticks;
        }

        public SensorType Type { get { return SensorType.Accelerometer; } }
        public void Stop() { acce.Stop(); }

        public S.Matrix Noise { get; set; }
        
        public TimeSpan TimeBetweenUpdates
        {
            get { return acce.TimeBetweenUpdates; }
        }
    }
}
